﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
//using Emgu.CV;


namespace Clothoid
{
    public partial class Form1 : Form
    {
        const double INFINITY = 100000000;
        const double DERIV_STEP_FOR_NEAREST_POINT = 1e-5;
        public const double PI = 3.141592653589793238463;
        
        public Form1()
        {
            InitializeComponent();
        }

        private void btnDraw_Click(object sender, EventArgs e)
        {
            //DrawClothoid((double)numB.Value, (double)numt1.Value, (double)numt2.Value, 10);
            


            panel1.Invalidate();
        }


        private void buttonDrawBt1t2_Click(object sender, EventArgs e)
        {
            panel1.Invalidate();
        }

        private void DrawClothoid(double B, double t1, double t2, int extendSize)
        {
            
        }

        private void panel1_Paint(object sender, PaintEventArgs e)
        {            
            double K1 = (double)numK1.Value;
            double K2 = (double)numK2.Value;
            double sign = 1; if (K1 > K2) sign = -1;
            double B = Math.Sqrt((double)numLength.Value / (PI * Math.Abs(K2-K1)));

            double t1, t2;
            t1 = K1 * B;
            t2 = K2 * B;


            numt1.Value = (decimal)t1;
            numt2.Value = (decimal)t2;

            numB.Value = (decimal)B;

            int extendSize = 10;
           
            double theta1 = (double)numTheta1.Value;
            double theta1computed = t1 *t1 *90 * sign;
            theta1computed = (int)theta1computed % 360; txtStartingAngle.Text = theta1computed.ToString();
            double deltaTheta = ((int)(theta1 - theta1computed) % 360);

            int xOrigin =  500;
            int yOrigin = panel1.Height / 2 ;
            
            System.Drawing.Pen penOriginal = new System.Drawing.Pen(System.Drawing.Color.Red);
            System.Drawing.Pen penRotated = new System.Drawing.Pen(System.Drawing.Color.BlueViolet);
            System.Drawing.Pen penTransformed = new System.Drawing.Pen(System.Drawing.Color.Tomato);

            System.Drawing.Graphics formGraphics = panel1.CreateGraphics();

            for (int i = -100; i < 100; i+=10)
            for (int j = -100; j < 100; j += 10)
            {
                formGraphics.DrawEllipse(penOriginal, xOrigin+ i * extendSize - 1, yOrigin - j * extendSize - 1, 2, 2);
            }
            penOriginal.Color = Color.Black;
            formGraphics.DrawLine(penOriginal, xOrigin - 100 * extendSize, yOrigin, xOrigin + 100 * extendSize, yOrigin);
            formGraphics.DrawLine(penOriginal, xOrigin, yOrigin - 100 * extendSize, xOrigin, yOrigin + 100 * extendSize);
            
            formGraphics.DrawRectangle(penOriginal, xOrigin-1, yOrigin-1, 2, 2);
            
            

            double R1 = (0.506 * t1 + 1) / (1.79 * t1 * t1 + 2.054 * t1 + Math.Sqrt(2));
            double A1 = 1 / (0.803 * t1 * t1 * t1 + +1.886 * t1 * t1 + +2.524 * t1 + 2);
            //double x1 = PI * B * (0.5 - R1 * Math.Sin(0.5 * PI * (A1 - t1 * t1)));
            //double y1 = PI * B * (0.5 - R1 * Math.Cos(0.5 * PI * (A1 - t1 * t1)));
            PointF p1 = computePointOfClothoid(B, t1, sign);
            double xRotated1 = p1.X * Math.Cos(deltaTheta / 180 * PI) - p1.Y * Math.Sin(deltaTheta / 180 * PI);
            double yRotated1 = p1.X * Math.Sin(deltaTheta / 180 * PI) + p1.Y * Math.Cos(deltaTheta / 180 * PI);
            double xTranslate = (double)numC1x.Value - xRotated1;
            double yTranslate = (double)numC1y.Value - yRotated1;

            double lastX = 0, lastY = 0;
            double lastXRotated = 0, lastYRotated = 0;
            double lastXTransformed = 0, lastYTransformed = 0;


            double tMin = t1;
            double tMax = t2;
            if (t1 > t2)
            {
                tMin = t2;
                tMax = t1;
            }
            for ( double t = t1; t*sign < t2*sign; t += 0.001*sign)
            {
                //double R = (0.506 * t + 1) / (1.79 * t * t + 2.054 * t + Math.Sqrt(2));
                //double A = 1 / (0.803 * t * t * t + +1.886 * t * t + +2.524 * t + 2);

                //double p.X = PI * B * (0.5 - R * Math.Sin(0.5 * PI * (A - t * t)));
                //double p.Y = PI * B * (0.5 - R * Math.Cos(0.5 * PI * (A - t * t)));
                PointF p = computePointOfClothoid(B, t, sign);
                if (lastX == 0) lastX = p.X;
                if (lastY == 0) lastY = p.Y;
                formGraphics.DrawLine(penOriginal, (int)(xOrigin + extendSize * lastX), (int)(yOrigin - extendSize * lastY),
                                    (int)(xOrigin + extendSize * p.X), (int)(yOrigin - extendSize * p.Y));

                double xRotated = p.X * Math.Cos(deltaTheta / 180 * PI) - p.Y * Math.Sin(deltaTheta / 180 * PI);
                double yRotated = p.X * Math.Sin(deltaTheta / 180 * PI) + p.Y * Math.Cos(deltaTheta / 180 * PI);
                if (lastXRotated == 0) lastXRotated = xRotated;
                if (lastYRotated == 0) lastYRotated = yRotated;
                formGraphics.DrawLine(penRotated, (int)(xOrigin + extendSize * lastXRotated), (int)(yOrigin - extendSize * lastYRotated),
                                    (int)(xOrigin + extendSize * xRotated), (int)(yOrigin - extendSize * yRotated));

                double xTransformed = xRotated + xTranslate;
                double yTransformed = yRotated + yTranslate;
                if (lastXTransformed == 0) lastXTransformed = xTransformed;
                if (lastYTransformed == 0) lastYTransformed = yTransformed;

                formGraphics.DrawLine(penTransformed, (int)(xOrigin + extendSize * lastXTransformed), (int)(yOrigin - extendSize * lastYTransformed),
                                    (int)(xOrigin + extendSize * xTransformed), (int)(yOrigin - extendSize * yTransformed));

                lastX = p.X;
                lastY = p.Y;
                lastXRotated = xRotated;
                lastYRotated = yRotated;
                lastXTransformed = xTransformed;
                lastYTransformed = yTransformed;
            }
            double startAngle = 90 * t1 * t1;

            penOriginal.Dispose();
            penRotated.Dispose();
            penTransformed.Dispose();

            formGraphics.Dispose();
        }

        private PointF computePointOfClothoid(double B, double t, double sign)
	    {
            double abst = Math.Abs(t);
            double R = (0.506 * abst + 1) / (1.79 * abst * abst + 2.054 * abst + Math.Sqrt(2));
            double A = 1 / (0.803 * abst * abst * abst + +1.886 * abst * abst + +2.524 * abst + 2);
            double x = PI * B * (0.5 - R * Math.Sin(0.5 * PI * (A - abst * abst))) * sign;
            double y = PI * B * (0.5 - R * Math.Cos(0.5 * PI * (A - abst * abst)));

            PointF p; 
            if (t < 0) p = new PointF(-(float)x, -(float)y);
            else p = new PointF((float)x, (float)y);
            return p;
            //return PointF vcg(	PI * B * (0.5 - R * sin(0.5 * PI * A - t * t)),
            //                    PI * B * (0.5 - R * cos(0.5 * PI * A - t * t)) );
	    }
        private void btnFindNearest_Click(object sender, EventArgs e)
        {
            float outsiteX = (float)numOutsiteX.Value;
            float outsiteY = (float)numOutsiteY.Value;

            double K1 = (double)numK1.Value;
            double K2 = (double)numK2.Value;
            double sign = 1; if (K1 > K2) sign = -1;

            double B = Math.Sqrt((double)numLength.Value / (PI * Math.Abs(K2 - K1)));
            numB.Value = (decimal)B;
            double t1 = (double)numK1.Value * B;
            double t2 = (double)numK2.Value *B;

            double theta1 = (double)numTheta1.Value;
            double theta1computed = t1 * t1 * 90 * sign;
            theta1computed = (int)theta1computed % 360; txtStartingAngle.Text = theta1computed.ToString();
            double deltaTheta = ((int)(theta1 - theta1computed) % 360);

            double R1 = (0.506 * t1 + 1) / (1.79 * t1 * t1 + 2.054 * t1 + Math.Sqrt(2));
            double A1 = 1 / (0.803 * t1 * t1 * t1 + +1.886 * t1 * t1 + +2.524 * t1 + 2);
            PointF p1 = computePointOfClothoid(B, t1, sign);
            double xRotated1 = p1.X * Math.Cos(deltaTheta / 180 * PI) - p1.Y * Math.Sin(deltaTheta / 180 * PI);
            double yRotated1 = p1.X * Math.Sin(deltaTheta / 180 * PI) + p1.Y * Math.Cos(deltaTheta / 180 * PI);
            double xTranslate = (double)numC1x.Value - xRotated1;
            double yTranslate = (double)numC1y.Value - yRotated1;

            double tLeft = t1;
            double tRight = t2;

            double t = t1;
            double lastMinimumDistance = INFINITY;
            for (int i = 0; i < 10000; i++)
            {           
                PointF leftPoint = computePointOfClothoid(B, tLeft, sign);
                PointF rightPoint = computePointOfClothoid(B, tRight, sign);

                PointF rotatedLeftPoint = new PointF((float)(leftPoint.X * Math.Cos(deltaTheta / 180 * PI) - leftPoint.Y * Math.Sin(deltaTheta / 180 * PI)),
                                                    (float)(leftPoint.X * Math.Sin(deltaTheta / 180 * PI) + leftPoint.Y * Math.Cos(deltaTheta / 180 * PI)));
                PointF translatedLeftPoint = new PointF(rotatedLeftPoint.X + (float)xTranslate, rotatedLeftPoint.Y + (float)yTranslate);

                PointF rotatedRightPoint = new PointF((float)(rightPoint.X * Math.Cos(deltaTheta / 180 * PI) - rightPoint.Y * Math.Sin(deltaTheta / 180 * PI)),
                                                    (float)(rightPoint.X * Math.Sin(deltaTheta / 180 * PI) + rightPoint.Y * Math.Cos(deltaTheta / 180 * PI)));
                PointF translatedRightPoint = new PointF(rotatedRightPoint.X + (float)xTranslate, rotatedRightPoint.Y + (float)yTranslate);


                //double distanceToLeft = Math.Sqrt((leftPoint.X - outsiteX) * (leftPoint.X - outsiteX) + (leftPoint.Y - outsiteY) * (leftPoint.Y - outsiteY));
                //double distanceToRight = Math.Sqrt((rightPoint.X - outsiteX) * (rightPoint.X - outsiteX) + (rightPoint.Y - outsiteY) * (rightPoint.Y - outsiteY));

                double distanceToLeft = Math.Sqrt((translatedLeftPoint.X - outsiteX) * (translatedLeftPoint.X - outsiteX) + (translatedLeftPoint.Y - outsiteY) * (translatedLeftPoint.Y - outsiteY));
                double distanceToRight = Math.Sqrt((translatedRightPoint.X - outsiteX) * (translatedRightPoint.X - outsiteX) + (translatedRightPoint.Y - outsiteY) * (translatedRightPoint.Y - outsiteY));


                double minimumDistance = 0;
                if (distanceToLeft < distanceToRight)
                {
                    minimumDistance = distanceToLeft;
                    if ((lastMinimumDistance - minimumDistance) < 1e-4 && t != tLeft)
                        break;
                    
                    t = tLeft;
                    tRight = (tLeft + tRight) / 2;
                }
                else
                {
                    minimumDistance = distanceToRight;
                    if ((lastMinimumDistance - minimumDistance) < 1e-4 && t != tRight)
                        break;
                    
                    t = tRight;
                    tLeft = (tLeft + tRight)/2;
                }
                if(lastMinimumDistance < minimumDistance)
                    MessageBox.Show("Last < mini");
                //if (Math.Abs(lastMinimumDistance - minimumDistance) < 1e-4) 
                //    break;
                lastMinimumDistance = minimumDistance;
                //printf("t = %d \t distanctLeft %.3f %.3f %.3f  t=%.3f\n", i, distanceToLeft, distanceToRight, derivative, t);
                //if (Math.Abs(derivative) < 1e-4) break;
                //t = t - derivative;
            }

            PointF nearestPoint = computePointOfClothoid(B, t, sign);
            PointF rotatedNearestPoint = new PointF((float)(nearestPoint.X * Math.Cos(deltaTheta / 180 * PI) - nearestPoint.Y * Math.Sin(deltaTheta / 180 * PI)),
                                                  (float)(nearestPoint.X * Math.Sin(deltaTheta / 180 * PI) + nearestPoint.Y * Math.Cos(deltaTheta / 180 * PI)));
            PointF translatedNearestPoint = new PointF(rotatedNearestPoint.X + (float)xTranslate, rotatedNearestPoint.Y + (float)yTranslate);


            System.Drawing.Pen penLine = new System.Drawing.Pen(System.Drawing.Color.Brown);
            System.Drawing.Graphics formGraphics = panel1.CreateGraphics();
            int xOrigin = 500;
            int yOrigin = panel1.Height / 2;
            int extendSize = 10;
            //formGraphics.DrawLine(penLine, xOrigin + outsiteX * extendSize, yOrigin - outsiteY * extendSize, xOrigin + nearestPoint.X * extendSize, yOrigin - nearestPoint.Y * extendSize);
            //formGraphics.DrawLine(penLine, xOrigin + outsiteX * extendSize, yOrigin - outsiteY * extendSize, xOrigin + rotatedNearestPoint.X * extendSize, yOrigin - rotatedNearestPoint.Y * extendSize);
            formGraphics.DrawLine(penLine, xOrigin + outsiteX * extendSize, yOrigin - outsiteY * extendSize, xOrigin + translatedNearestPoint.X * extendSize, yOrigin - translatedNearestPoint.Y * extendSize);
            txtNearest.Text = string.Format("X ={0} Y = {1}", translatedNearestPoint.X, translatedNearestPoint.Y);
           // MessageBox.Show(string.Format("t = {0}, x = %f, y= %f", t, nearestPoint.X, nearestPoint.Y));
        }

    }
}

